{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from PIL import Image\n",
    "import numpy as np\n",
    "from skimage.morphology import skeletonize\n",
    "from skimage import img_as_bool\n",
    "image_path = '/home/shaohang/Downloads/2024_small.jpg'\n",
    "\n",
    "# # Allow image size to exceed default PIL limit\n",
    "# ImageFile.LOAD_TRUNCATED_IMAGES = True\n",
    "Image.MAX_IMAGE_PIXELS = None  # Caution: Only do this if you're sure it's safe!\n",
    "\n",
    "\n",
    "# Load the image\n",
    "original_image = Image.open(image_path)\n",
    "\n",
    "# Convert image to grayscale\n",
    "gray_image = original_image.convert('L')\n",
    "\n",
    "# Threshold the image\n",
    "threshold = 128\n",
    "binary_image = gray_image.point(lambda x: 255 if x > threshold else 0, '1')\n",
    "\n",
    "# Convert binary image to boolean array\n",
    "binary_array = img_as_bool(binary_image)\n",
    "\n",
    "# Perform skeletonization\n",
    "skeleton = skeletonize(binary_array)\n",
    "\n",
    "# Extract the coordinates\n",
    "y, x = np.where(skeleton)\n",
    "coordinates = list(zip(x, y))\n",
    "coordinates = np.array(coordinates)\n",
    "\n",
    "import matplotlib.pyplot as plt\n",
    "plt.scatter(coordinates[:,0], coordinates[:,1])\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import cv2\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "# Load the image\n",
    "image_path = '/home/shaohang/Downloads/4.jpg'\n",
    "image = cv2.imread(image_path, cv2.IMREAD_GRAYSCALE)\n",
    "\n",
    "print('Image shape:', image.shape)\n",
    "\n",
    "# Define the coordinates of the top-left and bottom-right corners of the square\n",
    "top_left_corner = (0, 8000)\n",
    "bottom_right_corner = (9000, 14882)\n",
    "\n",
    "# Make a copy of the original image to draw on\n",
    "image_with_square = image.copy()\n",
    "\n",
    "# Draw a black rectangle (square) on the image\n",
    "# (0, 0, 0) is the color of the square (black) and -1 specifies that the square should be filled\n",
    "cv2.rectangle(image_with_square, top_left_corner, bottom_right_corner, (0, 0, 0), -1)\n",
    "\n",
    "# Define the coordinates of the top-left and bottom-right corners of the square\n",
    "top_left_corner = (14000, 0)\n",
    "bottom_right_corner = (26883, 1900)\n",
    "\n",
    "# Draw a black rectangle (square) on the image\n",
    "# (0, 0, 0) is the color of the square (black) and -1 specifies that the square should be filled\n",
    "cv2.rectangle(image_with_square, top_left_corner, bottom_right_corner, (0, 0, 0), -1)\n",
    "\n",
    "# Display the image with the black square\n",
    "plt.figure(figsize=(10, 10))\n",
    "plt.imshow(image_with_square, cmap='gray')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "meter_per_pixel = (0.35 / (1035 - 621)) / 1.169\n",
    "\n",
    "print(14882 * meter_per_pixel)\n",
    "print(26883 * meter_per_pixel)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "meter_per_pixel = (0.35 / (1035 - 621)) / 1.169\n",
    "\n",
    "# Threshold the image to get binary image\n",
    "_, binary_image = cv2.threshold(image_with_square, 128, 255, cv2.THRESH_BINARY)\n",
    "\n",
    "# Find the white pixel coordinates\n",
    "white_pixels = np.column_stack(np.where(binary_image == 255))\n",
    "\n",
    "# If the y-coordinate should not be flipped, we can use the original white_pixels without transformation\n",
    "original_coordinates = white_pixels - [14882 / 2, 26883 / 2]\n",
    "\n",
    "downsample_factor = 1000\n",
    "downsampled_coordinates = original_coordinates[::downsample_factor]\n",
    "downsampled_coordinates = downsampled_coordinates.astype(np.float32)\n",
    "downsampled_coordinates *= meter_per_pixel\n",
    "print(downsampled_coordinates.shape)\n",
    "plt.figure(figsize=(10, 10))\n",
    "plt.scatter(downsampled_coordinates[:, 0], downsampled_coordinates[:, 1], s=0.1)\n",
    "plt.axes().set_aspect('equal')\n",
    "plt.show()\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for coo in downsampled_coordinates:\n",
    "    if coo[1] < (-4.95) and coo[1] > (-5.015):\n",
    "        print(coo)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from pyproj import Proj, Transformer\n",
    "\n",
    "def transform_points(cartesian_points):\n",
    "    # Define the WGS84 geographic coordinate system\n",
    "    proj_wgs84 = Proj(proj='latlong', datum='WGS84')\n",
    "\n",
    "    # Define a local Cartesian projection centered at 0 degrees lat, 0 degrees lon\n",
    "    proj_cartesian = Proj(proj='aeqd', lat_0=0, lon_0=0, datum='WGS84', units='m')\n",
    "\n",
    "    # Create a transformer to convert from Cartesian to WGS84 geographic coordinates\n",
    "    transformer = Transformer.from_proj(proj_cartesian, proj_wgs84)\n",
    "\n",
    "    # Convert the Cartesian coordinates to latitude and longitude\n",
    "    geo_points = [transformer.transform(y, x) for x, y in cartesian_points]\n",
    "\n",
    "    # Prepare the OSM XML content\n",
    "    osm_xml_content = '<osm version=\"0.6\" generator=\"Custom Script\">\\n'\n",
    "    for idx, (lat, lon) in enumerate(geo_points, start=1):\n",
    "        osm_xml_content += f'  <node id=\"{-idx}\" lat=\"{lat}\" lon=\"{lon}\" />\\n'\n",
    "    osm_xml_content += '</osm>'\n",
    "\n",
    "    # write the OSM XML content to a file\n",
    "    with open('road_2024.osm', 'w') as file:\n",
    "        file.write(osm_xml_content)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "transform_points(downsampled_coordinates)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# List of Cartesian coordinates to be converted\n",
    "cartesian_points = [(3, 1), \n",
    "                    (2, 1), \n",
    "                    (1, 1), \n",
    "                    (1, 2),\n",
    "                    (1, 3),\n",
    "                    (3, 0),\n",
    "                    (2, 0),\n",
    "                    (1, 0),\n",
    "                    (0, 0),\n",
    "                    (0, 1),\n",
    "                    (0, 2),\n",
    "                    (0, 3)]\n",
    "\n",
    "transform_points(cartesian_points)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "\n",
    "points = [[3, 0],\n",
    "          [2.5, 0],\n",
    "          [2, 0],\n",
    "          [1.5, 0],\n",
    "          [1, 0],\n",
    "          [0.35, 0],\n",
    "          [0, 0],\n",
    "          [0, 0.35],\n",
    "          [3, 0.35],\n",
    "          [2.5, 0.35],\n",
    "          [2, 0.35],\n",
    "          [1.5, 0.35],\n",
    "          [1, 0.35],\n",
    "          [0.35, 0.35],\n",
    "          [0, 1],\n",
    "          [0, 1.5],\n",
    "          [0, 2],\n",
    "          [0, 2.5],\n",
    "          [0, 3],\n",
    "          [0, 3.65],\n",
    "          [0, 4],\n",
    "          [0.35, 1],\n",
    "          [0.35, 1.5],\n",
    "          [0.35, 2],\n",
    "          [0.35, 2.5],\n",
    "          [0.35, 3],\n",
    "          [0.35, 3.65],\n",
    "          [0.35, 4],\n",
    "          [0.35, 4.35],\n",
    "          [0, 4.35],\n",
    "          [-0.35, 4],\n",
    "          [-0.8, 4],\n",
    "          [-1.3, 4],\n",
    "          [-1.65, 4],\n",
    "          [-0.35, 4.35],\n",
    "          [-0.8, 4.35],\n",
    "          [-1.3, 4.35],\n",
    "          [-1.65, 4.35],\n",
    "          [-2, 4.35],\n",
    "          [-2, 4],\n",
    "          [-1.65, 3.65],\n",
    "          [-1.65, 3],\n",
    "          [-1.65, 2.5],\n",
    "          [-1.65, 2],\n",
    "          [-1.65, 1.5],\n",
    "          [-1.65, 1],\n",
    "          [-1.65, 0.35],\n",
    "          [-1.65, 0],\n",
    "          [-2, 3.65],\n",
    "          [-2, 3],\n",
    "          [-2, 2.5],\n",
    "          [-2, 2],\n",
    "          [-2, 1.5],\n",
    "          [-2, 1],\n",
    "          [-2, 0.35],\n",
    "          [-2, 0],\n",
    "          [-1.65, -0.35],\n",
    "          [-1.65, -1],\n",
    "          [-1.65, -1.25],\n",
    "          [-2, -0.35],\n",
    "          [-2, -1],\n",
    "          [-2, -1.25],\n",
    "          [-1.3, 0],\n",
    "          [-0.8, 0],\n",
    "          [-0.35, 0],\n",
    "          [-1.3, -0.35],\n",
    "          [-0.8, -0.35],\n",
    "          [-0.35, -0.35],\n",
    "          [0, -0.35],\n",
    "          [3, -0.35],\n",
    "          [2.5, -0.35],\n",
    "          [2, -0.35],\n",
    "          [1.5, -0.35],\n",
    "          [1, -0.35],\n",
    "          [0.35, -0.35],\n",
    "          [0.35, -1],\n",
    "          [0.35, -1.25],\n",
    "          [0, -1],\n",
    "          [0, -1.25],\n",
    "          [-0.35, -1],\n",
    "          [-0.35, -1.25],\n",
    "          [-1.3, -1],\n",
    "          [-1.3, -1.25],\n",
    "          [-0.35, 0.35],\n",
    "          [-0.35, 1],\n",
    "          [-0.35, 1.5],\n",
    "          [-0.35, 2],\n",
    "          [-0.35, 2.5],\n",
    "          [-0.35, 3],\n",
    "          [-0.35, 3.65],\n",
    "          [-1.3, 0.35],\n",
    "          [-1.3, 1],\n",
    "          [-1.3, 1.5],\n",
    "          [-1.3, 2],\n",
    "          [-1.3, 2.5],\n",
    "          [-1.3, 3],\n",
    "          [-1.3, 3.65],\n",
    "          [-0.8, 0.35],\n",
    "          [-0.8, 3.65]\n",
    "          ]\n",
    "\n",
    "fig, ax = plt.subplots()\n",
    "fig.set_size_inches(10, 10)\n",
    "ax.plot([x for x, y in points], [y for x, y in points], 'o')\n",
    "\n",
    "transform_points(points)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import lanelet2\n",
    "from pyproj import Proj, transform\n",
    "\n",
    "# Define the projector with the origin\n",
    "projector = lanelet2.projection.UtmProjector(lanelet2.io.Origin(0, 0))\n",
    "\n",
    "# Load the map using Lanelet2\n",
    "map = lanelet2.io.load(\"test.osm\", projector)\n",
    "\n",
    "# for points in map.pointLayer:\n",
    "#     print(points)\n",
    "\n",
    "lanelet_1 = map.laneletLayer[1]\n",
    "lanelet_2 = map.laneletLayer[2]\n",
    "\n",
    "for point in list(lanelet_2.centerline):\n",
    "    print(point.x, point.y)\n",
    "\n",
    "print()\n",
    "for point in list(lanelet_1.centerline):\n",
    "    print(point.x, point.y)\n",
    "\n",
    "# traffic_rules = lanelet2.traffic_rules.create(lanelet2.traffic_rules.Locations.Germany,\n",
    "#                                                   lanelet2.traffic_rules.Participants.Vehicle)\n",
    "# routing_graph = lanelet2.routing.RoutingGraph(map, traffic_rules)\n",
    "# route = routing_graph.getRoute(lanelet_1, lanelet_2)\n",
    "# for lanelet_relation in route.followingRelations(lanelet_1):\n",
    "#     print(lanelet_relation.lanelet.id)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "a = \"1,2\"\n",
    "len(a)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "points = []\n",
    "# read txt file of tuples\n",
    "def read_txt(file_path):\n",
    "    with open(file_path, 'r') as file:\n",
    "        content = file.read()\n",
    "    content = content.split('\\n')\n",
    "    points = []\n",
    "    for tup_str in content:\n",
    "        # tup_str = tup_str[1:-1]\n",
    "        tup = tup_str.split(',')\n",
    "        tup = [float(x) for x in tup]\n",
    "        points.append(tup)\n",
    "    points = np.array(points)\n",
    "    return points\n",
    "\n",
    "file_path = '/home/shaohang/Downloads/adjusted_coordinates.txt'\n",
    "points = read_txt(file_path)\n",
    "plt.scatter(points[:, 0], points[:, 1], s=0.1)\n",
    "plt.axes().set_aspect('equal')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "new_points = []\n",
    "for point in points:\n",
    "    if (point[0] > 0) and (point[1] > 400):\n",
    "        continue\n",
    "    if (point[0] < -500) and (point[1] < -200):\n",
    "        continue\n",
    "    new_points.append(point)\n",
    "new_points = np.array(new_points) * 0.01 \n",
    "new_points = new_points[::2]\n",
    "plt.plot(9.75, 1.16, \"ro\")\n",
    "plt.plot(6.2, -4.7, \"ro\")\n",
    "plt.scatter(new_points[:, 0], new_points[:, 1], s=0.1)\n",
    "plt.axes().set_aspect('equal')\n",
    "plt.figure(dpi=300, figsize=(30, 30))\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "transform_points(new_points)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "iss",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.18"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
